/**
 * @file           : DMMotor.h
 * @author         : Star_Plucking
 * @brief          : None
 * @date           : 25-1-9 下午4:12
 * @lastEditor     :
 * @lastEditTime   : 25-1-9 下午4:12
 */
#pragma once

#include <connection_guard.h>
#include <sheriffos.h>

namespace device::dm_motors {
constexpr float kRadius2Angle = 180.0F / 3.1415926F;
constexpr float kRadius2RPM = 9.5492965855137201461330258023509F; ///< 60 / (2 * PI)
constexpr float kAngle2Radius = 3.1415926F / 180.0F;
constexpr float kRPM2Radius = 0.10471975511965977461542144610932F; ///< 2 * PI / 60

enum class DamiaoMotorState { kEnable, kDisable, kLost };

enum class DamiaoMotorModeType { MIT, PosSpeed, SpeedOnly };

enum class DamiaoMotorErrorCode : uint8_t {
    Disabled = 0x0,
    Enabled = 0x01,
    OverVoltage = 0x8,
    UnderVoltage = 0x9,
    OverCurrent = 0xA,
    MOS_OverTemperature = 0xB,
    RotorOverTemperature = 0xC,
    ConnectLost = 0xD,
    EncoderError = 0xE
};
} // namespace device::dm_motors

namespace device::dm_motors {
[[clang::always_inline]] static float uint2float(int num, float min, float max, int bits) {
    float span = max - min;
    float res = (static_cast<float>(num) * span / static_cast<float>((1 << bits) - 1)) + min;
    LimitMaxMin(res, max, min);
    return res;
}

[[clang::always_inline]] static int float2uint(float num, float min, float max, int bits) {
    LimitMaxMin(num, max, min);
    float span = max - min;
    return static_cast<int>((num - min) * static_cast<float>((1 << bits) - 1) / span);
}
} // namespace device::dm_motors

namespace device::dm_motors {
class Encoder {
protected:
    uint16_t m_kp = 0; ///< 电机MIT参数
    uint16_t m_ki = 0; ///< 电机MIT参数
    float m_RawAngle = 0; ///< (单位：角度)电机编码器返回角0-360（不含角度偏移）
    float m_Speed = 0; ///< (单位：rpm)电机编码器返回速度
    float m_Torque = 0; ///< (单位：NM)电机编码器返回力矩
    uint8_t m_MOS_Temp = 0; ///< (单位：摄氏度)驱动上MOS的平均温度
    uint8_t m_Rotor_Temp = 0; ///<(单位：摄氏度)转子温度
    uint64_t m_LastUpdateTime = 0; ///< (单位：毫秒) 上次更新时间

    int32_t m_Round_Count = 0; ///< 电机旋转圈数(含角度偏移)
    float m_ContinuousAngle = 0; ///< (单位：角度)电机连续角度(含角度偏移)
    float m_LimitedAngle = 0; ///< (单位：角度)电机当前角度-180-180(含角度偏移)
    float m_AngleOffset = 0; ///< (单位：角度)电机角度偏移

    float m_LastAngle = 0; ///< 上次角度(含角度偏移)
    float m_GearRatio = 0; ///< 减速比
    float m_ShaftSpeed = 0; ///< (单位：rpm)电机输出轴速度

    float kEncoderAngle = 0; ///< (单位：角度)编码器角度
    float kEncoderCurrent = 0; ///< 电流转换系

    DamiaoMotorErrorCode m_error_code = {}; ///< 电机错误码
    float m_AngleMax{}; ///< 电机最大角度
    float m_SpeedMax{}; ///< 电机最大速度
    float m_TorqueMax{}; ///< 电机最大力矩

public:
    Encoder() = default;

    explicit Encoder(float GearRatio = 1.0F, float const AngleOffset = 0.0F, float AngleMax = 12.5F,
                     float SpeedMax = 30.0F, float TorqueMax = 10.0F)
        : m_AngleOffset(AngleOffset),
          m_GearRatio(GearRatio),
          m_AngleMax(AngleMax),
          m_SpeedMax(SpeedMax),
          m_TorqueMax(TorqueMax) {
        m_LastUpdateTime = units::unit_cast<uint64_t>(os::getTime());
    }

    ~Encoder() = default;

    void decode(uint8_t const* data) {
        m_LastAngle = m_RawAngle;
        m_LastUpdateTime = units::unit_cast<uint64_t>(os::getTime());

        m_error_code = static_cast<DamiaoMotorErrorCode>(static_cast<uint8_t>(data[0] >> 4));
        m_RawAngle = uint2float(data[1] << 8 | data[2], -m_AngleMax, m_AngleMax, 16) * kRadius2Angle;
        m_Speed = uint2float(data[3] << 4 | data[4] >> 4, -m_SpeedMax, m_SpeedMax, 12) * kRadius2RPM;
        m_Torque = uint2float((data[4] & 0x0F) << 8 | data[5], -m_TorqueMax, m_TorqueMax, 12);
        m_MOS_Temp = static_cast<uint8_t>(data[6]);
        m_Rotor_Temp = static_cast<uint8_t>(data[7]);

        m_LimitedAngle = m_RawAngle;

        if (m_RawAngle - m_LastAngle < -160) {
            m_Round_Count++;
        } else if (m_RawAngle - m_LastAngle > 160) {
            m_Round_Count--;
        }
        m_ContinuousAngle = m_LimitedAngle + static_cast<float>(m_Round_Count) * 360.0F;

        m_ShaftSpeed = m_Speed / m_GearRatio;
    }

    [[nodiscard]] float GetRawAngle() const { return m_RawAngle; }

    [[nodiscard]] float GetSpeed() const { return m_Speed; }

    [[nodiscard]] float GetTorque() const { return m_Torque; }

    [[nodiscard]] float GetLimitedAngle() const { return m_LimitedAngle; }

    [[nodiscard]] float GetLimitAngle() const { return m_LimitedAngle; }

    [[nodiscard]] float GetContinuesAngel() const { return m_ContinuousAngle; }

    [[nodiscard]] float GetTemp() const { return m_Rotor_Temp; }

    [[nodiscard]] uint64_t GetLastUpdateTime() const { return m_LastUpdateTime; }

    [[nodiscard]] float GetGearRation() const { return m_GearRatio; }

    [[nodiscard]] float GetShaftSpeed() const { return m_ShaftSpeed; }

    [[nodiscard]] DamiaoMotorErrorCode GetErrorCode() const { return m_error_code; }

    [[nodiscard]] float GetAngleMax() const { return m_AngleMax; }

    [[nodiscard]] float GetAngleOffset() const { return m_AngleOffset; }

    [[nodiscard]] float GetSpeedMax() const { return m_SpeedMax; }

    [[nodiscard]] float GetTorqueMax() const { return m_TorqueMax; }
};
} // namespace device::dm_motors

namespace device::dm_motors {
class Motor {
private:
    DamiaoMotorState m_State{};
    DamiaoMotorState m_StateConnected{};
    DamiaoMotorModeType m_Mode{};
    float m_output = 0;
    uint32_t m_ID{};
    os::Can* m_can{};
    FDCAN_TxHeaderTypeDef _txHeader{
        .Identifier = m_ID,
        .IdType = FDCAN_STANDARD_ID,
        .TxFrameType = FDCAN_DATA_FRAME,
        .DataLength = FDCAN_DLC_BYTES_8,
        .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
        .BitRateSwitch = FDCAN_BRS_OFF,
        .FDFormat = FDCAN_CLASSIC_CAN,
        .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
        .MessageMarker = 0,
    };
    uint32_t m_MST_ID = 0;

public:
    Encoder encoder;
    Motor() = default;

    /**
     * @brief 达妙电机构造函数
     * @param ID 电机ID
     * @param MST_ID 电机反馈数据帧ID
     * @param GearRatio 减速比(不含电机减速箱)
     * @param AngleOffset 角度偏移
     * @param can
     * @param AngleMax
     * @param SpeedMax
     * @param TorqueMax
     */
    explicit Motor(uint32_t const ID, uint32_t const MST_ID, float const GearRatio, float const AngleOffset,
                   os::Can* can, float AngleMax, float SpeedMax, float TorqueMax)
        : m_ID(ID), m_can(can), m_MST_ID(MST_ID), encoder(GearRatio, AngleOffset, AngleMax, SpeedMax, TorqueMax) {
        m_State = DamiaoMotorState::kLost;
        m_StateConnected = DamiaoMotorState::kLost;
        os::ConnectionGuard::addGuard([this] { TestLost(); });
    }

    ~Motor() = default;

    void Enable() {
        uint8_t data[8] = {};
        for (uint8_t& i : data) {
            i = 0xFF;
        }
        data[7] = 0xFC;
        if (m_State != DamiaoMotorState::kLost) m_State = DamiaoMotorState::kEnable;
        m_StateConnected = DamiaoMotorState::kEnable;
        m_can->transmit(_txHeader, data);
    }

    void Disable() {
        uint8_t data[8] = {};
        for (uint8_t& i : data) {
            i = 0xFF;
        }
        data[7] = 0xFD;
        if (m_State != DamiaoMotorState::kLost) m_State = DamiaoMotorState::kDisable;
        m_StateConnected = DamiaoMotorState::kDisable;
        m_can->transmit(_txHeader, data);
    }

    void ClearError() const {
        uint8_t data[8] = {};
        for (uint8_t& i : data) {
            i = 0xFF;
        }
        data[7] = 0xFB;
        m_can->transmit(_txHeader, data);
    }

    /**
     * @brief MIT模式发送
     * @param Angle 角度期望(单位：度)
     * @param Speed 速度期望(单位：rpm)
     * @param Kp 角度增益
     * @param Kd 速度增益
     * @param Torque 力矩期望(单位：Nm)
     * @note 当使用角度期望时Kd不可以为0
     */
    void SetSendMIT(float Angle, float Speed, float Kp, float Kd, float Torque) {
        uint8_t data[8] = {};
        Angle *= kAngle2Radius;
        Speed *= kRPM2Radius;
        uint16_t pos_tmp = float2uint(Angle, -encoder.GetAngleMax(), encoder.GetAngleMax(), 16);
        uint16_t speed_tmp = float2uint(Speed, -encoder.GetSpeedMax(), encoder.GetSpeedMax(), 12);
        uint16_t kp_tmp = float2uint(Kp, 0.f, 500.f, 16);
        uint16_t kd_tmp = float2uint(Kd, 0.f, 5.f, 16);
        uint16_t torque_tmp = float2uint(Torque, -encoder.GetTorqueMax(), encoder.GetTorqueMax(), 12);
        data[0] = pos_tmp >> 8;
        data[1] = pos_tmp;
        data[2] = speed_tmp >> 4;
        data[3] = (speed_tmp & 0xF) << 4 | (kp_tmp >> 8);
        data[4] = kp_tmp;
        data[5] = kd_tmp >> 4;
        data[6] = (kd_tmp & 0xF) << 4 | (torque_tmp >> 8);
        data[7] = static_cast<uint8_t>(torque_tmp & 0xFF);
        _txHeader.Identifier = m_ID;
        m_can->transmit(_txHeader, data);
    }

    void SetSendSpeed(float Speed) {
        uint8_t data[8] = {};
        memcpy(data, &Speed, 4);
        _txHeader.Identifier = m_ID + 0x200;
        m_can->transmit(_txHeader, data);
    }

    void SetSendAngleSpeed(float Angle, float Speed) {
        uint8_t data[8] = {};
        memcpy(data, &Speed, 4);
        memcpy(data + 4, &Angle, 4);
        _txHeader.Identifier = m_ID + 0x100;
        m_can->transmit(_txHeader, data);
    }

    void TestLost() {
        if (HAL_GetTick() - encoder.GetLastUpdateTime() > 200) {
            m_State = DamiaoMotorState::kLost;
            m_StateConnected = DamiaoMotorState::kDisable;
        } else {
            m_State = m_StateConnected;
        }
    }

    [[nodiscard]] bool IsDisable() const { return m_State == DamiaoMotorState::kDisable; }

    [[nodiscard]] bool IsLost() const {
        return m_State == DamiaoMotorState::kLost;
    }

    void Decode(uint16_t const ID, uint8_t const* data) {
        if (ID == m_MST_ID) {
            encoder.decode(data);
        }
    }
};
} // namespace device::dm_motors